#ifndef _TOOL_PID_H__
#define _TOOL_PID_H__

#include "includes.h"
class PIDBase{
	public:
	float MODE;//目标值
	float Target;//目标值
	float Current;//当前值
	float Out;//解算后的输出值
	float PTerm,ITerm,DTerm;//PID各个分量输出值
	float Kp, Ki, Kd;//PID各个参数
	float Rpi, Rii, Rdi, Ti;//最小值
	float Rpa, Ria, Rda, Ta;//最大值
	float DeadZone;//死区
	short counter;//计数
	float now_error,last_error;//偏差

	PIDBase(){;}
	void SetPIDBase(float m_Kp, float m_Ki, float m_Kd, float m_Target,
		            float m_Rpi, float m_Rpa, float m_Rii, float m_Ria, float m_Rdi, float m_Rda, 
					float m_Ti, float m_Ta, float m_DeadZone);
	void SetTarget(float target);
	void SetTargetOffset(float target);
	void SetCurrent(float current);
	void SetPIDPara(float m_Kp, float m_Ki, float m_Kd);
	void SetTargetMaxMin( float m_Ti, float m_Ta);
	void SetPMaxMin(float m_Rpi, float m_Rpa);
	void SetIMaxMin(float m_Rii, float m_Ria);
	void SetDMaxMin(float m_Rdi, float m_Rda);
	void SetDeadZone(float m_DeadZone);
};

class PIDTimer
{
public:
	float dt;
	u32   last_time;//上次系统时间记录
	unsigned char UpdataTimeStamp(void);
};

class PID:public PIDBase,public PIDTimer
{
public:

	PID(){;};
	void SetPID(float m_Kp, float m_Ki, float m_Kd, float m_Target,
		        float m_Rpi,float m_Rii,float m_Rdi,  
		        float m_Rpa,float m_Ria,float m_Rda, 
			    float m_Ti, float m_Ta,float m_DeadZone);
	void AdjustPID(void);
private:
	struct filter_struct
	{
		unsigned char num[3];
		float filterbuff[3][5];//滤波器的数组BUFF
	}filterstruct;

};

class FUZZYPID:public PIDBase,public PIDTimer
{
public:
    float NB_PB,NM_PM,NS_PS;
	float NB_PB_kp,NM_PM_kp,NS_PS_kp;
	float NB_PB_ki,NM_PM_ki,NS_PS_ki;
	float NB_PB_kd,NM_PM_kd,NS_PS_kd;
	float Omega,Omega_Offset;
	float errorC,Kec;
	
    FUZZYPID(){;};
    void Set_FUZZYPID(float m_Kp, float m_Ki, float m_Kd, float m_Target,\
		 float m_Rpi, float m_Rpa, float m_Rii, float m_Ria,\
		 float m_Rdi, float m_Rda, float m_Ti, float m_Ta, float m_DeadZone,\
		 float m_NB_PB, float m_NM_PM, float m_NS_PS,\
		 float m_NB_PB_kp,float m_NM_PM_kp, float m_NS_PS_kp,\
		 float m_NB_PB_ki,float m_NM_PM_ki, float m_NS_PS_ki,\
		 float m_NB_PB_kd,float m_NM_PM_kd, float m_NS_PS_kd); 
	void AdjustPID(void);
	void Defuzzification(float error,float errorC);
	void SetOmega(float data,unsigned char dir)
	{
		Omega = (data - Omega_Offset);
		if(dir) Omega = -Omega;
	};
	void SetOmegaOffset(float data){
		Omega_Offset = data;
	};

	void SetFuzzy(float mode,float _NB_PB_kx1,float _NM_PM_kx2,float _NS_PS_kx3){
		switch((u8)mode){
			case 0:NB_PB = _NB_PB_kx1; 	NM_PM = _NM_PM_kx2;	 NS_PS = _NS_PS_kx3;break;
			case 1:NB_PB_kp = _NB_PB_kx1; NM_PM_kp = _NM_PM_kx2; NS_PS_kp = _NS_PS_kx3;break;
			case 2:NB_PB_ki = _NB_PB_kx1; NM_PM_ki = _NM_PM_kx2; NS_PS_ki = _NS_PS_kx3;break;
			case 3:NB_PB_kd = _NB_PB_kx1; NM_PM_kd = _NM_PM_kx2; NS_PS_kd = _NS_PS_kx3;break;
			default:break;
			}
		};
private:
	struct filter_struct
	{
		unsigned char num[3];
		float filterbuff[3][5];//滤波器的数组BUFF
	}filterstruct;
};





/****************************************分error正负的PID******************************************/



class PN_PIDBase{
	public:
	float MODE;//目标值
	float Target;//目标值
	float Current;//当前值
	float Out;//解算后的输出值
	float PTerm,ITerm,DTerm;//PID各个分量输出值
	float Kpp, Kip, Kdp;//正PID各个参数  	now_error = Target - Current;>0+deadzone
	float Kpn, Kin, Kdn;//负PID各个参数			now_error = Target - Current;<0-deadzone
	float Rpi, Rii, Rdi, Ti;//最小值
	float Rpa, Ria, Rda, Ta;//最大值
	float DeadZone;//死区
	short counter;//计数
	float now_error,last_error;//偏差
	float Omega,Omega_Offset;
	PN_PIDBase(){;}
	void SetPIDBase(float m_Kpp,  float m_Kip,  float m_Kdp, 
							float m_Kpn,  float m_Kin,  float m_Kdn, float m_Target,
				         float m_Rpi, float m_Rpa, float m_Rii, float m_Ria, float m_Rdi, float m_Rda, 
				         float m_Ti,  float m_Ta,  float m_DeadZone);
	void SetTarget(float target);
	void SetTargetOffset(float target);
	void SetCurrent(float current);
	void SetPIDPara(float m_Kpp, float m_Kip, float m_Kdp, float m_Kpn, float m_Kin, float m_Kdn);
	void SetTargetMaxMin( float m_Ti, float m_Ta);
	void SetPMaxMin(float m_Rpi, float m_Rpa);
	void SetIMaxMin(float m_Rii, float m_Ria);
	void SetDMaxMin(float m_Rdi, float m_Rda);
	void SetDeadZone(float m_DeadZone);
};

class PN_PID:public PN_PIDBase,public PIDTimer
{
public:
	PN_PID(){;};
	void SetPID(float m_Kpp, float m_Kip, float m_Kdp, 
				float m_Kpn, float m_Kin, float m_Kdn, float m_Target,
		        float m_Rpi,float m_Rii,float m_Rdi,  
		        float m_Rpa,float m_Ria,float m_Rda, 
			    float m_Ti, float m_Ta,float m_DeadZone);
	void AdjustPID(void);
	void SetOmega(float data,unsigned char dir)
	{
		Omega = (data - Omega_Offset);
		if(dir) Omega = -Omega;
	};
private:
	struct filter_struct
	{
		unsigned char num[3];
		float filterbuff[3][5];//滤波器的数组BUFF
	}filterstruct;
};




/***************************************辅助打击预测FUZZYPID******************************************/
class CVFUZZYPID:public PIDBase,public PIDTimer
{
public:
    float NB_PB,NM_PM,NS_PS;
	float NB_PB_kp,NM_PM_kp,NS_PS_kp;
	float NB_PB_ki,NM_PM_ki,NS_PS_ki;
	float NB_PB_kd,NM_PM_kd,NS_PS_kd;
	float Omega,Omega_Offset;
	float errorC,Kec;
	
    CVFUZZYPID(){;};
    void Set_FUZZYPID(float m_Kp, float m_Ki, float m_Kd, float m_Target,\
		 float m_Rpi, float m_Rpa, float m_Rii, float m_Ria,\
		 float m_Rdi, float m_Rda, float m_Ti, float m_Ta, float m_DeadZone,\
		 float m_NB_PB, float m_NM_PM, float m_NS_PS,\
		 float m_NB_PB_kp,float m_NM_PM_kp, float m_NS_PS_kp,\
		 float m_NB_PB_ki,float m_NM_PM_ki, float m_NS_PS_ki,\
		 float m_NB_PB_kd,float m_NM_PM_kd, float m_NS_PS_kd); 
	void AdjustPID(void);
	void Defuzzification(float error,float errorC);
	void SetOmega(float data,unsigned char dir)
	{
		Omega = (data - Omega_Offset);
		if(dir) Omega = -Omega;
	};
	void SetOmegaOffset(float data){
		Omega_Offset = data;
	};

	void SetFuzzy(float mode,float _NB_PB_kx1,float _NM_PM_kx2,float _NS_PS_kx3){
		switch((u8)mode){
			case 0:NB_PB = _NB_PB_kx1; 	NM_PM = _NM_PM_kx2;	 NS_PS = _NS_PS_kx3;break;
			case 1:NB_PB_kp = _NB_PB_kx1; NM_PM_kp = _NM_PM_kx2; NS_PS_kp = _NS_PS_kx3;break;
			case 2:NB_PB_ki = _NB_PB_kx1; NM_PM_ki = _NM_PM_kx2; NS_PS_ki = _NS_PS_kx3;break;
			case 3:NB_PB_kd = _NB_PB_kx1; NM_PM_kd = _NM_PM_kx2; NS_PS_kd = _NS_PS_kx3;break;
			default:break;
			}
		};
private:
	struct filter_struct
	{
		unsigned char num[3];
		float filterbuff[3][5];//滤波器的数组BUFF
	}filterstruct;
};

/***************************************PN_FUZZYPID******************************************/
class PN_FUZZYPID:public PIDBase,public PIDTimer
{
public:
    float NB,PB,NM,PM,NS,PS;
	float NB_kp,PB_kp,NM_kp,PM_kp,NS_kp,PS_kp;
	float NB_ki,PB_ki,NM_ki,PM_ki,NS_ki,PS_ki;
	float NB_kd,PB_kd,NM_kd,PM_kd,NS_kd,PS_kd;
	float Omega,Omega_Offset;
	float errorC,Kec;
	
    PN_FUZZYPID(){;};
    void Set_FUZZYPID(float m_Kp, float m_Ki, float m_Kd, float m_Target,\
		 float m_Rpi, float m_Rpa, float m_Rii, float m_Ria,\
		 float m_Rdi, float m_Rda, float m_Ti, float m_Ta, float m_DeadZone,\
		 float m_PB, float m_PM, float m_PS,\
		 float m_NB, float m_NM, float m_NS,\
		 float m_PB_kp,float m_PM_kp, float m_PS_kp,float m_NS_kp,float m_NM_kp,float m_NB_kp,\
		 float m_PB_ki,float m_PM_ki, float m_PS_ki,float m_NS_ki,float m_NM_ki,float m_NB_ki,\
		 float m_PB_kd,float m_PM_kd, float m_PS_kd,float m_NS_kd,float m_NM_kd,float m_NB_kd\
			 );
	void AdjustPID(void);
	void Defuzzification(float error,float errorC);
	void SetOmega(float data,unsigned char dir)
	{
		Omega = (data - Omega_Offset);
		if(dir) Omega = -Omega;
	};
	void SetOmegaOffset(float data){
		Omega_Offset = data;
	};

	void SetFuzzy(float mode,float _PB_kx1,float _PM_kx2,float _PS_kx3,float _NS_kx4,float _NM_kx5,float _NB_kx6 ){
		switch((u8)mode){
			case 0:PB = _PB_kx1; PM = _PM_kx2; PS = _PS_kx3; NS = _NS_kx4; NM = _NM_kx5; NB = _NB_kx6;break;
			case 1:PB_kp = _PB_kx1; PM_kp = _PM_kx2; PS_kp = _PS_kx3; NS_kp = _NS_kx4; NM_kp = _NM_kx5; NB_kp = _NB_kx6;break;
			case 2:PB_ki = _PB_kx1; PM_ki = _PM_kx2; PS_ki = _PS_kx3; NS_ki = _NS_kx4; NM_ki = _NM_kx5; NB_ki = _NB_kx6;break;
			case 3:PB_kd = _PB_kx1; PM_kd = _PM_kx2; PS_kd = _PS_kx3; NS_kd = _NS_kx4; NM_kd = _NM_kx5; NB_kd = _NB_kx6;break;
			default:break;
			}
		};
private:
	struct filter_struct
	{
		unsigned char num[3];
		float filterbuff[3][5];//滤波器的数组BUFF
	}filterstruct;
};




#endif
